#include "wiicom.h"

WIICOM wiicom;
JOY_GAUGE joy_gauge;
float rot_magnitude = 0.0f;
G3::VEC2F rot_angle;
bool motion_sensing_on = true;
float battery_level = 1.0f;

//==============================================================================
// A Wiimote Communication class constructor
//==============================================================================
WIIMOUSE::WIIMOUSE () {
	State = 0;
	Sensitivity = G3CS_3DM_PlanarSensitivity;

	SetScreenSize (800, 600);
}

WIICOM::WIICOM () {
	log = &G3::MainLog;

	delta_time = 0.0f;

	found = 0;
	connected = 0;
	timeout = 5;

	wiimotes_total = 1;

    online = false;
    thread = NULL;
}

JOY_GAUGE::JOY_GAUGE () {
	mag = 0.0f;
	ang = 0.0f;
}

//==============================================================================
// A Wiimote Communication class destructor
//==============================================================================
WIICOM::~WIICOM () {
	stop ();
}

JOY_GAUGE::~JOY_GAUGE () {
	// nop;
}

/**
 Apply Laplacian smoothing, using the umbrella operator
 to approximate the Laplacian.
*/
float smooth_laplace (float a) {
	static float arch [10];
	static int num_arch = 0;
	static int num_arch2 = 0;
	double umbrella = 0.0;
	float result = 0.0f;

	if (num_arch < 10) {
		arch [num_arch] = a;
		num_arch++;
	} else {
		arch [num_arch2] = a;

		umbrella = 0.5 * (arch [num_arch2 - 2] + arch [num_arch2 - 1]) - arch [num_arch2];
		result = arch [num_arch2] + G3CS_WII_DepthNoiseReduction * umbrella;
		arch [num_arch2] = result;

		num_arch2 = (num_arch2 + 1) % 10;

		return result;
	}
	return a;
}

float average_filter (float a) {
	static float arch [3];
	static int num_arch = 0;
	static int num_arch2 = 0;
	float result = 0.0f;

	// Less than 3 samples ?
	if (num_arch < 3) {
		arch [num_arch] = a;
		num_arch++;
	} else {
		arch [num_arch2] = a;
		result = a - (a - (arch [0] + arch [1] + arch [2]) / 3.0f) * G3CS_WII_DepthNoiseReduction;
		arch [num_arch2] = result;
		num_arch2 = (num_arch2 + 1) % 3;
		return result;
	}
	return a;
}

float lowpass_filter (float a) {
	return average_filter (a);
//	return smooth_laplace (a);
}

void WIIMOUSE::SetMode (uint mode) {
	Mode = mode;
}

void WIIMOUSE::SetScreenSize (int Width, int Height) {
	ScreenWidth = Width;
	ScreenHeight = Height;
	ScreenCenter = G3::VEC3I (Width / 2, Height / 2, 0);
}

void WIIMOUSE::SetPosition (int x, int y, int z) {
	LastPosition = Position;
	Position = G3::VEC3I (x, y, z);
}

void WIIMOUSE::Center () {
     ScreenCenter = G3::VEC3I (ScreenWidth / 2, ScreenWidth / 2, 0);

     SetPosition (ScreenCenter.x, ScreenCenter.y, 0);
}

void JOY_GAUGE::PreCalc () {
	int i = 0;
	float a = 0.0f;
	int radius = (Rect.Right - Rect.Left) / 2;

	for (i=0; i<30; i++) {
		verts [i] = G3::VEC2I ((int) (cosf (a) * radius), (int) (sinf (a) * radius));
		//! \note This should be changed if the number of vertices changes
		a += G3_PI / 15.0f;
	}
}

void JOY_GAUGE::Draw (float aElapsedTime) {
    if (!Visible)
        return;

    glPushMatrix ();
    glDisable (GL_TEXTURE_2D);

	int i;
	int radius = (Rect.Right - Rect.Left) / 2;

	glColor3f (0.8f, 0.8f, 0.8f);

	glBegin (GL_LINE_LOOP);
	for (i=0; i<30; i++)
		glVertex2i (Rect.Left + verts [i].x, -Rect.Top - verts [i].y);
	glEnd ();

	G3::VEC2I vec;

	if (ang > G3_EPSILON || ang < G3_EPSILON)
		vec = G3::VEC2I (mag * sinf (G3_DEG2RAD (ang)) * radius, mag * cosf (G3_DEG2RAD (ang)) * radius);

	glColor3f (0.0f, 0.8f, 0.0f);

	glBegin (GL_LINES);
	glVertex2i (Rect.Left, -Rect.Top);
	glVertex2i (Rect.Left + vec.x, -Rect.Top + vec.y);
	glEnd ();

    glEnable (GL_TEXTURE_2D);

    glPopMatrix ();
}

void WIICOM_THREAD::operator()() {
	G3::TIMER timer;
	float elapsed = 0.0f;

    if (wiicom.log)
        wiicom.log->Report ("Wiimote communications thread online..\n");

    wiicom.online = true;

	// Connect to wiimotes
	if (wiicom.connect () < 0)
		return;

	timer.Start ();

    while (wiicom.online) {
		// Update wiimote input data
		if (!G3CS_3DM_UseNormalMouse) {
			elapsed = timer.CheckTime ();
			elapsed /= 1000.0f;
			wiicom.update (elapsed);
		}

        // Idle for some time
        boost::thread::yield ();
    }
}

void WIICOM::start (int num_wiimotes) {
	wiimotes_total = num_wiimotes;

	// Initialize wiiuse
	wiimotes = wiiuse_init (wiimotes_total);
/*
#ifdef WIN32
    if (log)
        log->Report ("WIICOM::start: Trying to use Microsoft Bluetooth Stack..\n");

    // Try to use the Microsoft bluetooth stack
    wiiuse_set_bluetooth_stack (wiimotes, wiimotes_total, WIIUSE_STACK_MS);
#endif
*/
    if (log)
        log->Report ("WIICOM::start: Starting the communications thread..\n");

    static WIICOM_THREAD wc;

    try {
        thread = new boost::thread (wc);
    } catch (...) {
        if (log)
            log->Report ("WIICOM::start: Failed to start the thread..\n");
    }
}

void WIICOM::stop () {
    online = false;

    if (thread != NULL) {
        if (log)
            log->Report ("WIICOM::stop: Joining the communications thread..\n");

        try {
            thread->join ();
            delete thread;
			wiiuse_cleanup (wiimotes, wiimotes_total);
        } catch (...) {
            if (log)
                log->Report ("WIICOM::stop: Failed to join the thread..\n");
        }

        thread = NULL;
    }
}

bool WIICOM::isOnline () {
    return online;
}

//==============================================================================
// Connects to available wiimotes
//==============================================================================
int WIICOM::connect () {
	int i;

	status = "Searching for wiimotes..";

	if (log)
		log->Report ("Searching for wiimotes..\n");
	// Find these devices (timeout is 5 seconds)
	found = wiiuse_find (wiimotes, wiimotes_total, timeout);
	// Found no devices ?
	if (!found) {
		if (log)
			log->Report ("Couldn't find any wiimotes..\n");
		status = "Couldn't find any wiimotes";
		G3CS_3DM_UseNormalMouse = true;
		return -1;
	}

	if (log)
		log->Report ("Trying to connect those wiimotes..\n");
	// Try to connect
	connected = wiiuse_connect (wiimotes, wiimotes_total);
	// Couldn't connect to any of them ?
	if (!connected) {
		if (log)
			log->Report ("Couldn't connect to any of the wiimotes..\n");
		status = "Couldn't connect to any of the wiimotes";
		G3CS_3DM_UseNormalMouse = true;
		return -2;
	}

	for (i=0; i<wiimotes_total; i++) {
		wiiuse_motion_sensing (wiimotes [i], 1);
		// Set a relatively high threshold for accelerometer orientation
		wiiuse_set_orient_threshold (wiimotes [i], 1.5f);
		// Enable IR tracking on all of them
//		wiiuse_set_aspect_ratio (wiimotes [i], WIIUSE_ASPECT_16_9);
//		wiiuse_set_ir_position (wiimotes [i], WIIUSE_IR_BELOW);
//		wiiuse_set_ir_vres (wiimotes [i], 1066, 600);//3 * wiimouse.ScreenWidth, 3 * wiimouse.ScreenHeight);
		wiiuse_set_ir_vres (wiimotes [i], 3 * wiimouse.ScreenWidth, 3 * wiimouse.ScreenHeight);
		wiiuse_set_ir (wiimotes [i], 1);
		// Set IR camera sensitivity
		wiiuse_set_ir_sensitivity (wiimotes [i], G3CS_WII_IRSensitivity);
		// Set the leds and rumble so that it's easy to tell which
		// wiimotes are connected..
		wiiuse_set_leds (wiimotes [i], WIIMOTE_LED_1);
		//wiiuse_rumble (wiimotes [i], 1);
	}

	status = "Connected to " + G3::IntegerToStr (connected) + " wiimotes";
	if (log)
		log->Report ("%s\n", status.c_str ());

	// Everything should be ok
	return 0;
}

//==============================================================================
// Updates the wiimote input
//==============================================================================
void WIICOM::update (double elapsedTime) {
    bool changes = false;

	delta_time += elapsedTime;

	// More than half a second?
	if (delta_time >= 500.0) {
		for (int i=0; i<wiimotes_total; i++)
			wiiuse_rumble (wiimotes [i], 0);
	}

	// An event has ocurred?
	if (wiiuse_poll (wiimotes, wiimotes_total)) {
		for (int i=0; i<wiimotes_total; i++) {
			// Check for a certain event
			switch (wiimotes [i]->event) {
				// A generic event?
				case WIIUSE_EVENT:
					handle_generic_event (wiimotes [i], elapsedTime);
					break;

				// A status event?
				case WIIUSE_STATUS:
					handle_ctrl_status (wiimotes [i]);
					break;

				// Disconnect?
				case WIIUSE_DISCONNECT:
				case WIIUSE_UNEXPECTED_DISCONNECT:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " was disconnected";
					changes = true;
					handle_disconnect (wiimotes [i]);
					break;

				// Some data is returned?
				case WIIUSE_READ_DATA:
					break;

				// Nunchuk was inserted?
				case WIIUSE_NUNCHUK_INSERTED:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " now has a nunchuk";

					if (wiimotes_total == 1)
						joy_gauge.Visible = true;

					changes = true;
					break;

				// Or removed?
				case WIIUSE_NUNCHUK_REMOVED:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " no longer has the nunchuk";

					if (wiimotes_total == 1)
						joy_gauge.Visible = false;

					changes = true;
					break;

				// Some kind of a classic control was inserted?
				case WIIUSE_CLASSIC_CTRL_INSERTED:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " now has a classic control";
					changes = true;
					break;

				// Or removed?
				case WIIUSE_CLASSIC_CTRL_REMOVED:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " no longer has the classic control";
					changes = true;
					break;

				// How can we draw with a guitar?
				case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " now has a Guitar Hero 3 control";
					changes = true;
					break;

				// Actually, don't care about it that much.
				case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
					status = "Wiimote no. " + G3::IntegerToStr (i) + " no longer has the Guitar Hero 3 control";
					changes = true;
					break;

				default: break;
			}

            if (changes && log)
                log->Report ("%s\n", status.c_str ());
		}
	}
}

//==============================================================================
// Handles a generic wiimote event
//==============================================================================
void WIICOM::handle_generic_event (wiimote *mote, double elapsedTime) {
	SDL_Event event;

	// Map A to Left mouse button
	if (IS_JUST_PRESSED (mote, WIIMOTE_BUTTON_A)) {
		event.type = SDL_MOUSEBUTTONDOWN;
		event.button.button = SDL_BUTTON_LEFT;

		G3_SETFLAG (wiimouse.State, SDL_BUTTON (SDL_BUTTON_LEFT));
		SigWiimouseDown (&wiimouse, &event, elapsedTime);
	} else if (IS_RELEASED (mote, WIIMOTE_BUTTON_A)) {
		event.type = SDL_MOUSEBUTTONUP;
		event.button.button = SDL_BUTTON_LEFT;

		G3_UNSETFLAG (wiimouse.State, SDL_BUTTON (SDL_BUTTON_LEFT));
		SigWiimouseUp (&wiimouse, &event, elapsedTime);
	}

	// Map B to Right mouse button
	if (IS_JUST_PRESSED (mote, WIIMOTE_BUTTON_B)) {
		event.type = SDL_MOUSEBUTTONDOWN;
		event.button.button = SDL_BUTTON_RIGHT;

		G3_SETFLAG (wiimouse.State, SDL_BUTTON (SDL_BUTTON_RIGHT));
		SigWiimouseDown (&wiimouse, &event, elapsedTime);
	} else if (IS_RELEASED (mote, WIIMOTE_BUTTON_B)) {
		event.type = SDL_MOUSEBUTTONUP;
		event.button.button = SDL_BUTTON_RIGHT;

		G3_UNSETFLAG (wiimouse.State, SDL_BUTTON (SDL_BUTTON_RIGHT));
		SigWiimouseUp (&wiimouse, &event, elapsedTime);
	}

	// Pressing Minus stops motion sensing - saves battery
	if (IS_JUST_PRESSED (mote, WIIMOTE_BUTTON_MINUS)) {
		motion_sensing_on = false;
		wiiuse_motion_sensing (mote, 0);
	}

	// Pressing Plus starts motion sensing again
	if (IS_JUST_PRESSED (mote, WIIMOTE_BUTTON_PLUS)) {
		motion_sensing_on = true;
		wiiuse_motion_sensing (mote, 1);
	}

	// Pressing Home resets the spline and camera
	if (IS_JUST_PRESSED (mote, WIIMOTE_BUTTON_HOME)) {
        // Lock the spline from rendering
        boost::mutex::scoped_lock lock( Spline.m_mutex );

        StartNew();
	}

	// Pressing 1 toggles drawing mode
	if (IS_JUST_PRESSED (mote, WIIMOTE_BUTTON_ONE))
		ToggleDrawMode();

	// Get the mouse coordinates by IR
	if (WIIUSE_USING_IR (mote)) {
		char num_dots = 0;

		// Count the visible IR dots
		for (int i=0; i<4; i++) {
			if (mote->ir.dot [i].visible)
				num_dots++;
		}

		// Only 1 dot visible so that we can't calculate the Z-coordinate?
		if (num_dots < 2) {
			// Then make sure that the accelerometers are turned on
			if (!WIIUSE_USING_ACC (mote))
				wiiuse_motion_sensing (mote, 1);
			else {
				// Use Y-axis acceleration for mouse Z-coordinate
				wiimouse.Position.z += mote->gforce.y;
			}
		} else {
			// Warp the mouse into the new position
			wiimouse.SetPosition (mote->ir.x, mote->ir.y, (int) (lowpass_filter (mote->ir.z - G3CS_WII_MinDepth) * G3CS_WII_DepthSensitivity));
//			if (mote->ir.z - G3CS_WII_MinDepth < 0.0f)
//				printf ("Wiimote pos %d %d %f is over the limit..\n", mote->ir.x, mote->ir.y, mote->ir.z);
		}

		status = G3::IntegerToStr (num_dots) + " IR dots visible";

		// Throw a mouse move signal
		event.type = SDL_MOUSEMOTION;
		SigWiimouseMove (&wiimouse, &event, elapsedTime);
	}

	// This wiimote has a nunchuk attached?
	if (mote->exp.type == EXP_NUNCHUK) {
		// Get its pointer
		struct nunchuk_t* nc = (nunchuk_t*) &mote->exp.nunchuk;

		joy_gauge.mag = nc->js.mag;
		joy_gauge.ang = nc->js.ang;

		rot_magnitude = nc->js.mag;

		if (rot_magnitude < G3CS_WII_NunchukNoiseLevel)
			rot_magnitude = 0.0f;

		if (nc->js.ang > G3_EPSILON || nc->js.ang < G3_EPSILON) {
			if (!IS_PRESSED (nc, NUNCHUK_BUTTON_Z)) {
				//rot_angle.x = cosf (G3_DEG2RAD (nc->js.ang)) * rot_magnitude;
				rot_angle.y = sinf (G3_DEG2RAD (nc->js.ang)) * rot_magnitude;
			} else {
				frame.Renderer.Camera.Move (G3::VEC3F (0.0f, 0.0f, 1.0f) * elapsedTime * cosf (G3_DEG2RAD (nc->js.ang)) * rot_magnitude * G3CS_WII_ZoomSensitivity);
			}
		}
	}
}

//==============================================================================
// Handles wiimote control status events
//==============================================================================
void WIICOM::handle_ctrl_status (wiimote *mote) {
	battery_level = mote->battery_level;
}

//==============================================================================
// Handles wiimote disconnect events
//==============================================================================
void WIICOM::handle_disconnect (wiimote *mote) {
	status = "Wiimote disconnected";
}

WIICOM::SIG_CONNECTION WIICOM::doOnWiimouseDown (WIIMOUSE_HANDLER aWiimouseHandler) {
	Assert (aWiimouseHandler, "WIICOM::doOnWiimouseDown: aWiimouseHandler argument is a NULL pointer");

	SIG_CONNECTION c = SigWiimouseDown.connect (aWiimouseHandler);
	ConnWiimouseDown.push_back (c);

	return c;
}

WIICOM::SIG_CONNECTION WIICOM::doOnWiimouseUp (WIIMOUSE_HANDLER aWiimouseHandler) {
	Assert (aWiimouseHandler, "WIICOM::doOnWiimouseUp: aWiimouseHandler argument is a NULL pointer");

	SIG_CONNECTION c = SigWiimouseUp.connect (aWiimouseHandler);
	ConnWiimouseUp.push_back (c);

	return c;
}

WIICOM::SIG_CONNECTION WIICOM::doOnWiimouseMove (WIIMOUSE_HANDLER aWiimouseHandler) {
	Assert (aWiimouseHandler, "WIICOM::doOnWiimouseMove: aWiimouseHandler argument is a NULL pointer");

	SIG_CONNECTION c = SigWiimouseMove.connect (aWiimouseHandler);
	ConnWiimouseMove.push_back (c);

	return c;
}

